/*
 * Copyright (c) 2006-2018, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2018-11-7      SummerGift   first version
 */
#include <stdint.h>
#include "plf.h"
#include "os_mem.h"

#include "co_printf.h"
#include "driver_pmu.h"
#include "jump_table.h"
#include "driver_uart.h"
#include "core_cm3.h"
#include "sys_utils.h"

#include "ble_stack.h"
#include "rtthread.h"
#include "rthw.h"

extern uint32_t system_get_pclk(void);
#define configCPU_CLOCK_HZ                  ( system_get_pclk() )
#define configTICK_RATE_HZ                  ( 100 )      //10ms
#define configSYSTICK_CLOCK_HZ configCPU_CLOCK_HZ
#define portNVIC_SYSTICK_CLK_BIT    ( 1UL << 2UL )

/* Constants required to manipulate the core.  Registers first... */
#define portNVIC_SYSTICK_CTRL_REG           ( * ( ( volatile uint32_t * ) 0xe000e010 ) )
#define portNVIC_SYSTICK_LOAD_REG           ( * ( ( volatile uint32_t * ) 0xe000e014 ) )
#define portNVIC_SYSTICK_CURRENT_VALUE_REG  ( * ( ( volatile uint32_t * ) 0xe000e018 ) )
#define portNVIC_SYSPRI2_REG                ( * ( ( volatile uint32_t * ) 0xe000ed20 ) )
/* ...then bits in the registers. */
#define portNVIC_SYSTICK_INT_BIT            ( 1UL << 1UL )
#define portNVIC_SYSTICK_ENABLE_BIT         ( 1UL << 0UL )
#define portNVIC_SYSTICK_COUNT_FLAG_BIT     ( 1UL << 16UL )
#define portNVIC_PENDSVCLEAR_BIT            ( 1UL << 27UL )
#define portNVIC_PEND_SYSTICK_CLEAR_BIT     ( 1UL << 25UL )

void test_log1(void)
{
    uart_putc_noint_no_wait(UART0, 'W');
}
void test_log2(void)
{
    uart_putc_noint_no_wait(UART0, 'U');
}


/* SysTick configuration */
void rt_hw_systick_init(void)
{
    portNVIC_SYSTICK_CTRL_REG = 0UL;
    portNVIC_SYSTICK_CURRENT_VALUE_REG = 0UL;

    /* Configure SysTick to interrupt at the requested rate. */
    portNVIC_SYSTICK_LOAD_REG = ( configSYSTICK_CLOCK_HZ / configTICK_RATE_HZ ) - 1UL;
    portNVIC_SYSTICK_CTRL_REG = ( portNVIC_SYSTICK_CLK_BIT | portNVIC_SYSTICK_INT_BIT | portNVIC_SYSTICK_ENABLE_BIT );

    NVIC_SetPriority(SysTick_IRQn, 4);
    NVIC_SetPriority(PendSV_IRQn, 4);
}

/**
 * This is the timer interrupt service routine.
 *
 */
void SysTick_Handler(void)
{
    /* enter interrupt */
    rt_interrupt_enter();
//    uart_putc_noint_no_wait(UART0, 't');
//    HAL_IncTick();
    rt_tick_increase();

    /* leave interrupt */
    rt_interrupt_leave();
}


/**
 * This function will initial STM32 board.
 */
__attribute__((weak)) void rt_hw_board_init()
{
    rt_hw_systick_init();
    uint8_t *p = os_malloc(17*1024);
    uint32_t begin = (uint32_t)p;
    if(begin & 0x03)
    {
        begin &= (~0x03);
        begin += 4;
    }

    /* Heap initialization */
#if defined(RT_USING_HEAP)
    rt_system_heap_init((void *)begin, (void *)(begin + 17*1024 -4));
#endif

    /* Board underlying hardware initialization */
#ifdef RT_USING_COMPONENTS_INIT
    rt_components_board_init();
#endif
}

#define SLOT_SIZE            625
#define MAX_CLOCK_TIME              ((1L<<28) - 1)
#define CLK_SUB(clock_a, clock_b)     ((uint32_t)(((clock_a) - (clock_b)) & MAX_CLOCK_TIME))
#define CLK_DIFF(clock_a, clock_b)     ( (CLK_SUB((clock_b), (clock_a)) > ((MAX_CLOCK_TIME+1) >> 1)) ?                      \
                          ((int32_t)((-CLK_SUB((clock_a), (clock_b))))) : ((int32_t)((CLK_SUB((clock_b), (clock_a))))) )

static volatile uint8_t app_user_rtos_wait_wakeup_end;
static sys_baseband_time_t rtos_sleep_time;
uint32_t uSysTickStopValue;
extern rt_uint32_t rt_thread_ready_priority_group;
__attribute__((section("ram_code"))) void rtos_baseband_restore_done(void)
{
#if 1
    sys_baseband_time_t current_time = system_get_baseband_time();
    int32_t diff_time;
    uint32_t ulCompleteTickPeriods, ulReloadValue;

    /* restore systick and pendSV */
    /* unit: us */
    diff_time = CLK_DIFF( (rtos_sleep_time.hs*SLOT_SIZE + rtos_sleep_time.hus)>>1, (current_time.hs*SLOT_SIZE + current_time.hus)>>1 );
    diff_time += 200;

    //co_printf("d:%d,%d,%d\r\n",diff_time,uSysTickStopValue,configSYSTICK_CLOCK_HZ);

    /* unit: sys_tick */
    ulReloadValue = diff_time*(configSYSTICK_CLOCK_HZ/1000000) + uSysTickStopValue;
    ulCompleteTickPeriods = ulReloadValue / (configSYSTICK_CLOCK_HZ / configTICK_RATE_HZ);
    ulReloadValue = ulReloadValue % (configSYSTICK_CLOCK_HZ / configTICK_RATE_HZ);

    //ulCompleteTickPeriods = diff_time / ( 1000000 / configTICK_RATE_HZ );
    //ulReloadValue = ( configSYSTICK_CLOCK_HZ / configTICK_RATE_HZ ) - system_get_pclk_config() * (diff_time % ( 1000000 / configTICK_RATE_HZ ));

    /* Make PendSV and SysTick the lowest priority interrupts. */
    NVIC_SetPriority(SysTick_IRQn, 4);
    NVIC_SetPriority(PendSV_IRQn, 4);

    //vTaskStepTick( ulCompleteTickPeriods );
    rt_tick_set(rt_tick_get()+ulCompleteTickPeriods );

//co_printf("%d,%d\r\n",ulCompleteTickPeriods,ulReloadValue);
    SysTick->LOAD  = (uint32_t)(ulReloadValue - 1UL);
    SysTick->VAL   = 0UL;
    SysTick->CTRL  = SysTick_CTRL_CLKSOURCE_Msk |
                     SysTick_CTRL_TICKINT_Msk   |
                     SysTick_CTRL_ENABLE_Msk;
    SysTick->LOAD = ( configSYSTICK_CLOCK_HZ / configTICK_RATE_HZ ) - 1UL;

    app_user_rtos_wait_wakeup_end = 1;
    NVIC_SetPriority(BLE_IRQn, 2);
#endif
}

#define pdMS_TO_TICKS( xTimeInMs ) ( ( rt_tick_t ) ( ( ( rt_tick_t ) ( xTimeInMs ) * ( rt_tick_t ) configTICK_RATE_HZ ) / ( rt_tick_t ) 1000 ) )
#define portSY_FULL_READ_WRITE      ( 15 )

__attribute__((section("ram_code"))) void rt_system_power_manager(void)
{
    rt_tick_t xExpectedIdleTime;

    rt_base_t level = rt_hw_interrupt_disable();
    rt_tick_t next_timout = rt_timer_next_timeout_tick();
    if( next_timout >= rt_tick_get() )
        xExpectedIdleTime = next_timout - rt_tick_get();
    else
        xExpectedIdleTime = next_timout + (RT_TICK_MAX - rt_tick_get()) + 1;
    rt_hw_interrupt_enable(level);

    if( xExpectedIdleTime >= 2 )
    {
        rt_enter_critical();

        int32_t sleep_duration;
        uint32_t sleep_max_dur;

        /* unit: us */
        sleep_max_dur = ( __jump_table.slp_max_dur >> 1 ) * SLOT_SIZE;
        /* unit: tick */
        sleep_max_dur = pdMS_TO_TICKS( sleep_max_dur / 1000 );
        if(sleep_max_dur < xExpectedIdleTime)
        {
            xExpectedIdleTime = sleep_max_dur;
        }

        /* this value should be taken as sleep delay */
        //if(xExpectedIdleTime > __jump_table.sleep_delay_for_os)
        {
            //xExpectedIdleTime -= __jump_table.sleep_delay_for_os;
        }
        /* unit: half slot */
        sleep_duration = ( ( xExpectedIdleTime * ( 1000000 / configTICK_RATE_HZ ) ) << 1 ) / SLOT_SIZE;
        sleep_duration -=3;

        /* Stop the SysTick momentarily.  The time the SysTick is stopped for
        is accounted for as best it can be, but using the tickless mode will
        inevitably result in some tiny drift of the time maintained by the
        kernel with respect to calendar time. */

        /* Enter a critical section but don't use the taskENTER_CRITICAL()
        method as that will mask interrupts that should exit sleep mode. */
        __disable_irq();
        __dsb( portSY_FULL_READ_WRITE );
        __isb( portSY_FULL_READ_WRITE );

        uSysTickStopValue = SysTick->VAL;
        rtos_sleep_time = system_get_baseband_time();

        //co_printf("r1:%d\r\n",sleep_duration);

        /* If a context switch is pending or a task is waiting for the scheduler
        to be unsuspended then abandon the low power entry. */
        if(( rt_thread_ready_priority_group != 0 )
           || ( ble_stack_sleep_time_calc( &sleep_duration ) == false )  )
        {
            //co_printf("N\r\n");
            __enable_irq();
        }
        //__enable_irq();
        else
        {
            //bool init_rf = false;
            GLOBAL_INT_DISABLE();
            ble_stack_set_sleep_dur(sleep_duration);
            ble_stack_enter_sleep();
            app_user_rtos_wait_wakeup_end = 0;
            NVIC_SetPriority(BLE_IRQn, 0);
            GLOBAL_INT_RESTORE();
            while(app_user_rtos_wait_wakeup_end == 0);
            ble_stack_schedule_backward();
        }

        rt_exit_critical();
    }
}


